/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_IMU Logger implementation.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: IMULogger.cpp,v 1.8 2009-06-29 11:29:50 morsko Exp $
 *
 */
#include "IMULogger.hpp"
#include "gimutils.h"



IMULogger::IMULogger(MaCI::IMU::CIMUClient *aClient,
                     std::string aMaCISLName):
  CSync(1, 1),               
  CThread(5),                
  Logger(),
  iIClient(aClient),
  iRunning(false),
  iRunningThreadStopped(true),
  iFile(NULL),
  iMaCISLName(aMaCISLName),
  iMenu(),
  iStoredValues(0)
{
  iMenu[std::string("Acceleration")] = true;
  iMenu[std::string("Angular Speed")] = true;
  iMenu[std::string("Angles")] = true;
  iMenu[std::string("Quaternion")] = true;
  iMenu[std::string("Orientation Matrix")] = true;

    /*
  iMenu = new wxMenu();
  // wxMenuItem *mItem = new wxMenuItem(NULL,0,"Log types");

  iMenu->Insert(0,0,"LOG TYPES");

  wxMenuItem *acc = iMenu->InsertCheckItem(1,1,"Acceleration");
  acc->Check(true);

  wxMenuItem *rot = iMenu->InsertCheckItem(2,2,"Rotatation");
  rot->Check(true);


  wxMenuItem *ang = iMenu->InsertCheckItem(3,3,"Angles");
  ang->Check(false);

  wxMenuItem *quat = iMenu->InsertCheckItem(4,4,"Quaternion");
  quat->Check(false);

  wxMenuItem *oM = iMenu->InsertCheckItem(5,5,"Orientation Matrix");
  oM->Check(false);
    */
}
//******************************************************************************

IMULogger::~IMULogger(){
  if(iRunning){
    StopLogging();
  }
 
}
//******************************************************************************
 
bool IMULogger::StartLogging(void)
{
  dPrint(1,"Start called");
  if(!iRunning){
    dPrint(1,"Start logging IMU");
    //Check for the logging types
    bool logAcc = iMenu[std::string("Acceleration")];//iMenu->IsChecked(1);
    bool logRot = iMenu[std::string("Angular Speed")];
    bool logAngle = iMenu[std::string("Angles")];
    bool logQuat = iMenu[std::string("Quaternion")];
    bool logOM = iMenu[std::string("Orientation Matrix")];
    dPrint(1,"%d %d %d %d %d ",logAcc,logRot,logAngle,logQuat,logOM);
    char buffer[256];

    //First open File
    if(iFilenameSet){  
      
      sprintf(buffer,"data/%s.txt",
              iFilename.c_str());
      iFile = fopen(buffer,"wt");  
    }else{
      gim::time tstamp(true);
      tm t;
      tstamp.getLocalTime(t);

      sprintf(buffer,"data/%s_%.2d_%.2d_%d_%.2d:%.2d:%.2d.txt",
              iMaCISLName.c_str(),
              t.tm_mday,
              t.tm_mon+1,
              t.tm_year+1900,
              t.tm_hour,
              t.tm_min,
              t.tm_sec);
      
      iFile = fopen(buffer,"wt");

    }

    //Search the group and instance names 
    std::string instance;
    std::string group;

    MaCI::MaCICtrl::SMaCISL sl(iMaCISLName);
    if (sl.IsValidFQMaCISL()) {
      group = sl.GetDeviceGroup();
      instance = sl.GetInstance();
    }

    //Set examples
    fprintf(iFile,"%%MaCI_IMU log file\n");
    fprintf(iFile,"%%Group: %s Instance: %s\n",group.c_str(),instance.c_str());
    fprintf(iFile,"%%command(0:Acceleration;1:Rotation;2:Angles;3:Quaternions;4:OrientationMatrix) timestamp_sec timestamp_usec [command specific]\n");
    fprintf(iFile,"%%(command 0 specific) accelerationX accelerationY accelerationZ\n");
    fprintf(iFile,"%%(command 1 specific) angularVelX angularVelY angularVelZ\n");
    fprintf(iFile,"%%(command 2 specific) roll pitch yaw\n");
    fprintf(iFile,"%%(command 3 specific) scalar x y z\n");
    fprintf(iFile,"%%(command 4 specific) e11 e12 e13 e21 e22 e23 e31 e32 e33\n");
    fprintf(iFile,"%%Orientation matrix:  --              --\n");
    fprintf(iFile,"%%                     | e11  e12   e13 |\n");
    fprintf(iFile,"%%                     | e21  e22   e23 |\n");
    fprintf(iFile,"%%                     | e31  e32   e33 |\n");
    fprintf(iFile,"%%                     --              --\n");
    fprintf(iFile,"\n");

    iRunning = true;
    //Finally run threads
    if(logAcc){
      RunThread(0);
    }
    if(logRot){
      RunThread(1);
    }
    if(logAngle){
      RunThread(2);
    }
    if(logQuat){
      RunThread(3);
    }
    if(logOM){
      RunThread(4);
    }
    return true;


  }else{
    dPrint(1,"IMULogger already running");
  }

  return false;
}
//******************************************************************************
 
bool IMULogger::StopLogging(void)
{


  if(iRunning){
    dPrint(5,"Stop logging IMU");
    Lock();
    iRunning = false;
    Unlock();
    WaitThread(0);
    WaitThread(1);
    WaitThread(2);
    WaitThread(3); 
    WaitThread(4);
    /*   for(int i = 0; i < iMenu->GetMenuItemCount();i++){
      iMenu->Enable(i,true);
    }
    */
    fclose(iFile);

  }else{
    dPrint(1,"IMU Logger already stopped");
  }
  return false;
}
//******************************************************************************

int IMULogger::ThreadFunction(const int aThreadNumber)
{
  using namespace MaCI::IMU;
   /////////Search the group and instance names :)
    std::string instance;
    std::string group;

    MaCI::MaCICtrl::SMaCISL sl(iMaCISLName);
    if (sl.IsValidFQMaCISL()) {
      group = sl.GetDeviceGroup();
      instance = sl.GetInstance();
    }
    //////////


 

  if(aThreadNumber == 0){
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
    TAcceleration3d acceleration;  

    while(iRunning){

      if(iIClient->GetAccelerations(acceleration,
                                    tstamp,
                                    &seq,
                                    2000)){
        Lock();
        
        fprintf(iFile,"0 %u %u %f %f %f",
                tstamp.timestamp_s,
                tstamp.timestamp_us,
                acceleration.accelerationX,
                acceleration.accelerationY,
                acceleration.accelerationZ);

        
        fprintf(iFile,"\n");
        fflush(iFile);
        ++iStoredValues;
        Unlock();
      }
    } 
      
    return 0;
  }

  if(aThreadNumber == 1){
    
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
        TAngularVelocity3d av;  
    while(iRunning){

      if(iIClient->GetAngularVelocities(av,
                                tstamp,
                                &seq,
                                2000)){
        Lock();
        
        fprintf(iFile,"1 %u %u %f %f %f",
                tstamp.timestamp_s,
              tstamp.timestamp_us,
              av.wX,
              av.wY,
              av.wZ);

        
        fprintf(iFile,"\n");
        fflush(iFile);
        ++iStoredValues;
        Unlock();
      }
      
    }
    ownSleep_ms(100);
  }
  
  if(aThreadNumber == 2){
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
    TAngle3d angle;  
    while(iRunning){

      if(iIClient->GetAngles(angle,
                             tstamp,
                             &seq,
                             2000)){
        Lock();
        
        fprintf(iFile,"2 %u %u %f %f %f",
                tstamp.timestamp_s,
                tstamp.timestamp_us,
                angle.roll,
                angle.pitch,
                angle.yaw);

        
        fprintf(iFile,"\n");
        fflush(iFile);
        ++iStoredValues;
        Unlock();
      }
      
    }
   
  }

  if(aThreadNumber == 3){
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
    TQuaternion quaternion;  

    while(iRunning){
     
      if(iIClient->GetQuaternion(quaternion,
                                 tstamp,
                                 &seq,
                                 2000)){
        Lock();
        
        fprintf(iFile,"3 %u %u %f %f %f %f",
                tstamp.timestamp_s,
                tstamp.timestamp_us,
                quaternion.scalar,
                quaternion.x,
                quaternion.y,
                quaternion.z);
        
        fprintf(iFile,"\n");
        fflush(iFile);
        ++iStoredValues;
        Unlock();
      }
      
    }
  }

  if(aThreadNumber == 4){
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
    TOrientationMatrix orientationMatrix;

    while(iRunning){

      
      if(iIClient->GetOrientationMatrix(orientationMatrix,
                                        tstamp,
                                        &seq,
                                        2000)){
        Lock();
        
        fprintf(iFile,"4 %u %u %f %f %f %f %f %f %f %f %f",
                tstamp.timestamp_s,
                tstamp.timestamp_us,
                orientationMatrix.e11,
                orientationMatrix.e12,
                orientationMatrix.e13,
                orientationMatrix.e21,
                orientationMatrix.e22,
                orientationMatrix.e23,
                orientationMatrix.e31,
                orientationMatrix.e32,
                orientationMatrix.e33);

        
        fprintf(iFile,"\n");
        fflush(iFile);
        ++iStoredValues;
        Unlock();
      }
      
    }
   
  }



  return 0;

}
//******************************************************************************

std::string IMULogger::GetName(){
  return "IMU Logger";
}
//******************************************************************************

Logger::TMenu *IMULogger::GetMenu(void){
    return &iMenu;
}
//******************************************************************************

bool IMULogger::GetAdditionalInformation(std::string &aInformation){
  if(iRunning){
    Lock();
    char buffer[256];
    sprintf(buffer,"%u stored values",iStoredValues);
    aInformation.clear();
    aInformation.assign(buffer);
    Unlock();
    return true;
  }else{
    aInformation = "Not active";
    return true;
}
}

//******************************************************************************
bool IMULogger::SetMenuItem(std::string aItem, bool aValue){ 

  if(!iRunning){
    iMenu[aItem] =  aValue;
    return true;
  }else{
    return false;
  }

}
//******************************************************************************
//******************************************************************************
//******************************************************************************
